#include "motor.h"

/**
 * @brief 硬件映射，请根据主板引脚和drivers_pwm.h里的枚举修改
 * 
 */



// 电机速度限制
#define speed_limit 9000
int16 limit_speed = 400;

float speed_out_l=0;  //速度环输出
float speed_out_r=0;  //速度环输出

float lp=25;float li=2.5;float ld=6;
float rp=25;float ri=2.5;float rd=6;

int16 ex_speedl  = 0;int16 er1_speedl = 0;int16 er2_speedl = 0;int16 er3_speedl = 0;
int16 ex_speedr  = 0;int16 er1_speedr = 0;int16 er2_speedr = 0;int16 er3_speedr = 0;

float power_l=0;float power_r=0;

uint8 stop_flag = 0;
uint32 ting_cnt = 0;

int16 max_speed=135;int16 min_speed=85;

void moter_use(void)
{
        power_l=speed_out_l;
        power_r=speed_out_r;

        power_l = limit(power_l,9000); //输出限幅
        power_r = limit(power_r,9000);

//       if(stop_flag == 1)
//        {
//            power_l = 0;
//            power_r = 0;
//        }
        set_speed((int16)(power_l),(int16)(power_r));
        
}
void speed_pid(void)
{
        ex_speedl =  min_speed;
        ex_speedr =  min_speed;

//        if(stop_flag == 1)
//        {
//            ex_speedl = 0;
//            ex_speedr = 0;
//        }

        limit_ab(ex_speedl,-limit_speed,limit_speed);
        limit_ab(ex_speedr,-limit_speed,limit_speed);

        er3_speedl = er2_speedl;                     //记录误差  左电机
        er2_speedl = er1_speedl;
        er1_speedl = ex_speedl - getspeed_l;

        speed_out_l += lp*(er1_speedl - er2_speedl) + li*er1_speedl + ld*(er1_speedl- 2*er2_speedl + er3_speedl);  //增量式PID控制器

        er3_speedr = er2_speedr;             //记录误差  右电机
        er2_speedr = er1_speedr;
        er1_speedr = ex_speedr - getspeed_r;

        speed_out_r += rp*(er1_speedr - er2_speedr) + ri*er1_speedr + rd*(er1_speedr- 2*er2_speedr + er3_speedr);  //增量式PID控制器

        speed_out_l = (int16)limit(speed_out_l,9000);//最终输出占空比
        speed_out_r = (int16)limit(speed_out_r,9000);
}


/**
 * @brief 左右电机初始化
 * 
 */
void motor_init(void)
{
    gpio_init(MOTOR1_DIR, GPO, GPIO_HIGH,GPO_PUSH_PULL); // 电机1方向引脚初始化
    pwm_init(MOTOR1_PWM, 17000, 0); // 电机1PWM引脚初始化，频率17KHz，初始占空比0

    gpio_init(MOTOR2_DIR, GPO, GPIO_HIGH, GPO_PUSH_PULL); // 电机2方向引脚初始化
    pwm_init(MOTOR2_PWM, 17000, 0);                       // 电机2PWM引脚初始化，频率17KHz，初始占空比0
}



void set_speed(int speed_l,int speed_r)   //开环
{
    if(speed_l >= 0 )
    {
      if (speed_l > speed_limit )  //限幅
      {
          speed_l = speed_limit;
      }
      gpio_set_level(MOTOR1_DIR,1);
      pwm_set_duty(MOTOR1_PWM, speed_l);
    }
    else if(speed_l < 0 )
    {
      if (speed_l < -speed_limit )  //限幅
      {
         speed_l = -speed_limit;
       }
      gpio_set_level(MOTOR1_DIR,0);
      pwm_set_duty(MOTOR1_PWM, -speed_l);
    }
    if( speed_r >= 0)
    {
       if (speed_r > speed_limit )  //限幅
        {
            speed_r = speed_limit;
        }
       gpio_set_level(MOTOR2_DIR,1);
       pwm_set_duty(MOTOR2_PWM, speed_r);
    }
    else if(speed_r < 0)
     {
      if (speed_r < -speed_limit )  //限幅
        {
          speed_r = -speed_limit;
        }
      gpio_set_level(MOTOR2_DIR,0);
      pwm_set_duty(MOTOR2_PWM, -speed_r);
     }
}



